#include "get_omega.h"
#include "debug.h"

void get_omega::energy_buff(int direction, int buff_type, int show_num, float angle,  float time1, struct logo final_center_R)
{
	if (buff_type != NO_BUFF)
	{
		if (buff_type == SMALL_BUFF)//small buff -1
		{
			point0 = forecast_point(direction,angle, OMEGA, final_center_R);
		}
		else if (buff_type == BIG_BUFF)//big buff 1
		{
			//积分得到Δθ
			delta_theta = get_delta_x(time1);

			//根据方向确定Δθ的正负
			delta_theta = delta_theta * direction;
			point0 = forecast_point(direction, angle + delta_theta, 0, final_center_R);
		}
		else//no buff_type 0
		{
			point0 = forecast_point(direction, angle, 0, final_center_R);
		}
	}
}

void get_omega::forecast(Mat& image, float& yaw,  float& pitch, struct logo final_center_R, int show_num)
{
	float delta_x = point0.x - GWIDTH / 2.0;
	float delta_y = point0.y - GHEIGHT / 2.0;

	yaw = atan(delta_x * (inchPerPixWidth_ / focalLenth_)) * 57.2958;//0.0075 4.8/640
	pitch = atan(delta_y * (inchPerPixHeight_ / focalLenth_)) * 57.2958;//0.0075 3.6/480

#ifdef DEBUG

	circle(image, point0, 1, Scalar(0, 255, 0), 5);
	line(image, Point(0, image.size().height / 2), Point(image.size().width, image.size().height / 2), Scalar(124, 200, 150), 1);
	line(image, Point(image.size().width / 2, 0), Point(image.size().width / 2, image.size().height), Scalar(124, 200, 150), 1);
	line(image, point0, Point(image.size().width / 2, image.size().height / 2), Scalar(200, 150, 26), 1);

	//drawgraph(yaw, pitch, show_num);

	//imshow("graph_x", graph_x);
	//imshow("graph_y", graph_y);
#endif // DEBUG
}

Point get_omega::forecast_point(int direction, float angle, float omega, struct logo R)
{
	Point2f point;

	point.x = R.x + R.r * sin((angle + direction * omega * DELAY) / 180 * PI);
	point.y = R.y - R.r * cos((angle + direction * omega * DELAY) / 180 * PI);

	return point;
}

void get_omega::drawgraph(float point_x, float point_y, int show_num)
{
	graph_x.create(Size(1400, 480), CV_8UC3);
	graph_y.create(Size(1400, 480), CV_8UC3);

	Point point_1(show_num, 100 - point_x);//yaw
	Point point_2(show_num, 100 - point_y);//pitch

	point1.push_back(point_1);
	point2.push_back(point_2);

	if (point1.size() > 1)
	{
		line(graph_x, point1[point1.size() - 2], point1[point1.size() - 1], Scalar(0, 0, 255));
		line(graph_y, point2[point2.size() - 2], point2[point2.size() - 1], Scalar(0, 255, 0));
	}
}

float get_omega::get_delta_x(float time)
{
	float x = 0;
	x = -0.785 / 1.884 * cosf(1.884 * (time + DELAY)) + 1.305 * (time + DELAY) +
		0.785 / 1.884 * cosf(1.884 * time) - 1.306 * time;//���ٶȽ��ж�����

	return x * 360 / (2 * PI); //x�ĵ�λΪrad 1rad == 360 / (2 * PI)
}